//
//  Matrix.c
//  3DMathSDK
//
//  Created by admin on 16/7/6.
//  Copyright © 2016年 douyu. All rights reserved.
//

#include "Matrix.h"
#include <stdlib.h>
#include <math.h>

const Matrix kMatrixZero = {0, 0, NULL};
const Matrix kMatrix4Identity = {4, 4, (float[16]){
    1,0,0,0,
    0,1,0,0,
    0,0,1,0,
    0,0,0,1
}};

float squaref(float number);



Matrix MatrixMake(int row, int column, float data[row*column])
{
    float *inData = malloc(sizeof(float) * row * column);
    if (data != NULL) {
        for (int index = 0; index < row*column; index ++ ) {
            inData[index] = data[index];
        }
    }
    Matrix matrix = {row, column, inData};
    return matrix;
}

Matrix MatrixFromVector3(Vector3 vec)
{
    float *data = malloc(sizeof(float)*3);
    data[0] = vec.x;
    data[1] = vec.y;
    data[2] = vec.z;
    return MatrixMake(3, 1, data);
}

Vector3 Vector3FromMatrix(Matrix mat)
{
    Vector3 vec = kVector3Zero;
    if (mat.row == 3 && mat.column == 1) {
        vec.x = mat.data[0];
        vec.y = mat.data[1];
        vec.z = mat.data[2];
    }
    return vec;
}

bool isMatrixEqual(Matrix mat1, Matrix mat2)
{
    if (mat1.row == mat2.row && mat1.column == mat2.column) {
        for (int index = 0; index < mat1.row*mat1.column; index ++ ) {
            if (mat1.data[index] != mat2.data[index]) {
                return false;
            }
        }
        return true;
    } else {
        return false;
    }
}

bool isIdentityMatrix(Matrix matrix)
{
    if (matrix.row != matrix.column) {
        return false;
    }
    
    for (int row = 0, index = 0; row < matrix.row; row ++ ) {
        for (int column = 0; column < matrix.column; column ++ , index ++ ) {
            float value = *MatrixDataPointer(matrix, row+1, column+1);
            if (row == column) {
                if (value != 1.f) {
                    return false;
                }
            } else {
                if (value != 0.f) {
                    return false;
                }
            }
        }
    }
    return true;
}

bool isOrthogonalMatrix(Matrix mat1)
{
    Matrix trans = transpose(mat1);
    Matrix result = multiply(mat1, trans);
    return isIdentityMatrix(result);
}

Matrix multiply(Matrix mat1, Matrix mat2)
{
    // mat1列数和mat2行数要相同
    if (mat1.column != mat2.row) {
        return (Matrix){0,0};
    }
    
    Matrix result = {mat1.row, mat2.row};
    
    size_t size = sizeof(float)*result.row*result.column;
    float *data = (float*)malloc(size);
    for (int row = 0, index = 0; row < result.row; row ++ ) {
        for (int column = 0; column < result.column; column ++ , index ++ ) {
            float value = 0;
            for (int k = 0; k < mat1.column; k++ ) {
                value += mat1.data[row*mat1.row+k]*mat2.data[k*mat2.row+column];
            }
            data[index] = value;
        }
    }
    result.data = data;
    return result;
}

Matrix transpose(Matrix mat)
{
    Matrix result = {mat.column, mat.row};
    float *data = malloc(sizeof(float) * mat.column * mat.row);
    result.data = data;

    for (int row = 0; row < result.row; row ++ ) {
        for (int column = 0; column < result.column; column ++ ) {
            data[row*result.column + column] = mat.data[column*mat.column + row];
        }
    }
    return result;
}

Matrix adjointMatrix(Matrix mat)
{
    Matrix result = MatrixMake(mat.row, mat.column, NULL);
    for (int row = 0; row < mat.row; row ++ ) {
        for (int column = 0; column < mat.column; column ++ ) {
            float *toPointer = MatrixDataPointer(result, row+1, column+1);
            *toPointer = cofactor(mat, row+1, column+1);
        }
    }
    return transpose(result);
}

float determinant(Matrix mat)
{
    if (mat.row != mat.column) {
        return 0;
    }
    if (mat.row == 2) {
        // 递归结束
        return mat.data[0]*mat.data[3]-mat.data[1]*mat.data[2];
    }
    float result = 0;

    for (int column = 0; column < mat.column; column ++ ) {
        float fromValue = *MatrixDataPointer(mat, 1, column+1);
        float cofatorValue = cofactor(mat, 1, column+1);
        float value = fromValue*cofatorValue;
        result += value;
    }
    return result;
}

Matrix cofactorMatrix(Matrix mat, int rowT, int columnT)
{
    if (mat.column < 2 || mat.column < 2 || rowT == 0 || columnT == 0) {
        return kMatrixZero;
    }
    
    Matrix result = MatrixMake(mat.row-1, mat.column-1, NULL);
    
    for (int row = 0; row < result.row; row ++ ) {
        for (int column = 0; column < result.column; column ++ ) {
            float *toPointer = MatrixDataPointer(result, row+1, column+1);
            
            int row_offset = (row < rowT-1)?0:1;
            int column_offset = (column < columnT-1)?0:1;
            
            float *fromPointer = MatrixDataPointer(mat, row+row_offset+1, column+column_offset+1);
            
            *toPointer = *fromPointer;
        }
    }
//    printMatrix(result);
    return result;
}

float cofactor(Matrix mat, int rowT, int columnT)
{
    Matrix cMatrix = cofactorMatrix(mat, rowT, columnT);
    printMatrix(cMatrix);
    return determinant(cMatrix)*pow(-1, rowT+columnT);
}

Matrix inverseMatrix(Matrix mat)
{
    Matrix adjMatrix = adjointMatrix(mat);
    float determinantResult = determinant(mat);
    for (int index = 0; index < adjMatrix.row*adjMatrix.column; index ++ ) {
        adjMatrix.data[index] /= determinantResult;
    }
    return adjMatrix;
}

// 返回对应位置数值的指针
float *MatrixDataPointer(Matrix matrix, int row, int column)
{
    // 矩阵里计数从1开始，程序里从0开始
    if (row == 0 || column == 0) {
        return NULL;
    }
    return &matrix.data[(row-1)*matrix.column+(column-1)];
}

void printMatrix(Matrix mat)
{
    printf("row:%d column:%d\n", mat.row, mat.column);
    for (int row = 0; row < mat.row; row ++ ) {
        for (int column = 0; column < mat.column; column ++ ) {
            int index = row*mat.column+column;
            printf("%*.*f", -4, 0, mat.data[index]);
        }
        printf("\n");
    }
}

#pragma mark 旋转矩阵

// 绕单位向量旋转
Matrix rotateVector3Matrix(Vector3 vec, float angle)
{
    float x = vec.x;
    float y = vec.y;
    float z = vec.z;
    
    float data[3*3] =
    {
        squaref(x)*(1-cosf(angle))+cosf(angle), x*y*(1-cosf(angle))+z*sinf(angle), x*z*(1-cosf(angle))-y*sinf(angle),
        x*y*(1-cosf(angle))-z*sinf(angle), squaref(y)*(1-cosf(angle))+cosf(angle), y*z*(1-cosf(angle))+x*sinf(angle),
        x*z*(1-cosf(angle))+y*sinf(angle), y*z*(1-cosf(angle))-x*sinf(angle), squaref(z)*(1-cosf(angle))+cosf(angle)
    };
    
    Matrix result = MatrixMake(3, 3, data);
    return result;
}

#pragma mark - 缩放矩阵
Matrix scaleVector3Matrix(Vector3 vec, float k)
{
    float x = vec.x;
    float y = vec.y;
    float z = vec.z;
    
    float data[3*3] =
    {
        1+(k-1)*squaref(x), (k-1)*x*y, (k-1)*x*z,
        (k-1)*x*y, 1+(k-1)*squaref(y), (k-1)*y*z,
        (k-1)*x*z, (k-1)*z*y, 1+(k-1)*squaref(z)
    };
    Matrix result = MatrixMake(3, 3, data);
    return result;
}

#pragma mark - 投影矩阵
Matrix projectionVector3Matrix(Vector3 vec)
{
    float x = vec.x;
    float y = vec.y;
    float z = vec.z;
    
    float data[3*3] =
    {
        1-squaref(x), -1*x*y, -1*x*z,
        -1*x*y, 1-squaref(y), -1*y*z,
        -1*x*z, -1*z*y, 1-squaref(z)
    };
    Matrix result = MatrixMake(3, 3, data);
    return result;
}

#pragma makr - 工具函数
// 平方
float squaref(float number)
{
    return powf(number, 2);
}
